﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Drawing;
using System.Drawing.Imaging;
using System.IO;
using System.Threading;
using System.Windows.Forms;
using OpenNI;
using Mapenect.Data;

namespace Mapenect.KinectInterface
{
    class DataRetriever
    {
        private Context context;            // Wird für fast alle OpenNI Aktionen benötigt.
        private DepthGenerator depthGen;       // Erzeugt das Tiefenbild.
        private ImageGenerator rgbGen;         // Erzeugt das RGB Bild.
        private UserGenerator userGen;         // Erkennt ganze User und sucht nach Fokusgeste
        private Bitmap bitmap;              // Wird das Tiefenbild für die Anzeige aufnehmen.
        private bool isRunning = true;
        private Thread readData;
        private Thread jointData;
        private string mode = "depth";
        private PictureBox picBox;


        private SkeletonCapability skeletonCap;
        private PoseDetectionCapability poseCap;
        private string calibPose;

        private Dictionary<int, bool> logJointsForUser = new Dictionary<int, bool>();
        private DataContainer jointDataCollection;

        public DataRetriever(PictureBox picFrame, DataContainer dataCol)
        {
            if (picFrame == null)
                throw new NullReferenceException("DataRetriever benötigt eine Zeichenfläche.");
            else if (dataCol == null)
                throw new NullReferenceException("DataRetriever  benötigt eine Collection für die JointDaten.");

            this.picBox = picFrame;
            this.jointDataCollection = dataCol;
        }

        internal bool initializeOpenNI()
        {
            try
            {
                // Initialize the context from the configuration file
                this.context = new Context(@"..\..\data\openniconfig.xml");
                // Get the depth generator from the config file
                this.depthGen = context.FindExistingNode(NodeType.Depth) as DepthGenerator;
                this.rgbGen = context.FindExistingNode(NodeType.Image) as ImageGenerator;
                this.userGen = new UserGenerator(context);
                if (this.depthGen == null)
                {
                    throw new Exception(@"Error in Data\openniconfig.xml. No depth node found.");
                }
                if (this.rgbGen == null)
                {
                    throw new Exception(@"Error in Data\openniconfig.xml. No Image node found.");
                }
                if (this.userGen == null)
                {
                    throw new Exception(@"Error in Data\openniconfig.xml. No user node found.");
                }

                MapOutputMode mapMode = this.depthGen.MapOutputMode;
                this.bitmap = new Bitmap((int)mapMode.XRes, (int)mapMode.YRes, System.Drawing.Imaging.PixelFormat.Format24bppRgb);


                this.skeletonCap = userGen.SkeletonCapability;
                this.poseCap = userGen.PoseDetectionCapability;
                this.calibPose = userGen.SkeletonCapability.CalibrationPose;
                userGen.NewUser += new EventHandler<NewUserEventArgs>(user_NewUser);
                userGen.LostUser += new EventHandler<UserLostEventArgs>(user_LostUser);
                this.skeletonCap.SetSkeletonProfile(SkeletonProfile.All);
                this.skeletonCap.CalibrationStart += new EventHandler<CalibrationStartEventArgs>(SkeletonCapability_CalibrationStart);
                this.skeletonCap.CalibrationEnd += new EventHandler<CalibrationEndEventArgs>(SkeletonCapability_CalibrationEnd);
                this.poseCap.PoseDetected += new EventHandler<PoseDetectedEventArgs>(PoseDetectionCapability_PoseDetected);
                this.userGen.StartGenerating();
            }
            catch (Exception)
            {
                return false;
            }


            // Set the timer to update the depth image every 10ms
            //System.Threading.Timer timer = new System.Threading.Timer(timerCallback,null,0,10);
            readData = new Thread(UpdateContext);
            readData.Start();

            jointData = new Thread(GetJointData);
            jointData.Start();
            return true;
        }

        internal bool StopRetrievingData()
        {
            try
            {
                isRunning = false;
                readData.Join();
                jointData.Join();
                return true;
            }
            catch (Exception)
            {
                return false;
            }
        }

        internal void setCapturingMode(string mode)
        {
            this.mode = mode;
        }

        #region OpenNI Eventhandler

        internal delegate void LogEventHandler(Object sender, LogEventArgs e);
        internal event LogEventHandler LogCalibPoseDetected;
        protected void PoseDetectionCapability_PoseDetected(object sender, PoseDetectedEventArgs e)
        {
            LogCalibPoseDetected(this, new LogEventArgs(e.ID, e.Pose));
            this.poseCap.StopPoseDetection(e.ID);
            //this.skeleton.RequestCalibration(e.ID, true);
            logJointsForUser[e.ID] = true;
        }

        internal event LogEventHandler LogSkeletonCalibrationEnded;
        protected void SkeletonCapability_CalibrationEnd(object sender, CalibrationEndEventArgs e)
        {
            LogSkeletonCalibrationEnded(this, new LogEventArgs(e.ID, e.Success));
            if (e.Success)
            {
                this.skeletonCap.StartTracking(e.ID);
                logJointsForUser[e.ID] = true;
            }
            else
            {
                this.skeletonCap.RequestCalibration(e.ID, true);//StartPoseDetection("PSI", e.ID);
                logJointsForUser[e.ID] = false;
            }
        }

        internal event LogEventHandler LogSkeletonCalibrationStarted;
        protected void SkeletonCapability_CalibrationStart(object sender, CalibrationStartEventArgs e)
        {
            LogSkeletonCalibrationStarted(this, new LogEventArgs(e.ID));
            this.poseCap.StartPoseDetection("PSI", e.ID);
        }

        internal event LogEventHandler LogUserLost;
        protected void user_LostUser(object sender, UserLostEventArgs e)
        {
            LogUserLost(this, new LogEventArgs(e.ID));
            logJointsForUser.Remove(e.ID);
        }

        internal event LogEventHandler LogNewUser;
        protected void user_NewUser(object sender, NewUserEventArgs e)
        {
            LogNewUser(this, new LogEventArgs(e.ID));
            //this.pose.StartPoseDetection("Psi", e.ID);
            this.skeletonCap.RequestCalibration(e.ID, true);
            logJointsForUser.Add(e.ID, false);
        }
        #endregion

        #region Threads

        private void UpdateContext()
        {
            while (isRunning)
            {
                try
                {
                    this.context.WaitOneUpdateAll(depthGen);
                }
                catch (StatusException ex)
                {
                    //MessageBox.Show(OpenNI.WrapperUtils.GetErrorMessage(ex.Status));
                    Debug.WriteLine(OpenNI.WrapperUtils.GetErrorMessage(ex.Status));
                }
                catch (Exception ex)
                {
                    Debug.WriteLine(ex.Message);
                }

                switch (mode)
                {
                    case "rgb":
                        UpdateRGB();
                        break;
                    case "depth":
                    default:
                        UpdateDepth();
                        break;
                }

                //Thread.Sleep(10);
            }
        }

        private static List<string> GetJointIterator()
        {
            List<string> jointIterator = new List<string>();
            jointIterator.Add(SkeletonJoint.Head.ToString());
            jointIterator.Add(SkeletonJoint.LeftElbow.ToString());
            jointIterator.Add(SkeletonJoint.LeftFoot.ToString());
            jointIterator.Add(SkeletonJoint.LeftHand.ToString());
            jointIterator.Add(SkeletonJoint.LeftHip.ToString());
            jointIterator.Add(SkeletonJoint.LeftKnee.ToString());
            jointIterator.Add(SkeletonJoint.LeftShoulder.ToString());
            jointIterator.Add(SkeletonJoint.Neck.ToString());
            jointIterator.Add(SkeletonJoint.RightElbow.ToString());
            jointIterator.Add(SkeletonJoint.RightFoot.ToString());
            jointIterator.Add(SkeletonJoint.RightHand.ToString());
            jointIterator.Add(SkeletonJoint.RightHip.ToString());
            jointIterator.Add(SkeletonJoint.RightKnee.ToString());
            jointIterator.Add(SkeletonJoint.RightShoulder.ToString());
            jointIterator.Add(SkeletonJoint.Torso.ToString());
            return jointIterator;
        }

        private void GetJointData()
        {
            List<String> jointIterator = GetJointIterator();

            while (isRunning)
            {
                // Joints loggen
                foreach (var x in logJointsForUser)
                {
                    if (x.Value)
                    {
                        foreach (string jointName in jointIterator)
                        {
                            SkeletonJoint jointType = (SkeletonJoint)Enum.Parse(typeof(SkeletonJoint), jointName);
                            SkeletonJointTransformation joint = skeletonCap.GetSkeletonJoint(x.Key, jointType);

                            switch (x.Key)
                            {
                                case 1:
                                    jointDataCollection.WriteJoint(user.user1, jointType, joint);
                                    break;
                                case 2:
                                    jointDataCollection.WriteJoint(user.user2, jointType, joint);
                                    break;
                                case 3:
                                    jointDataCollection.WriteJoint(user.user3, jointType, joint);
                                    break;
                                case 4:
                                    jointDataCollection.WriteJoint(user.user4, jointType, joint);
                                    break;
                            }
                        }

                    }
                }

                Thread.Sleep(50);
            }
        }

        #endregion

        #region Image Updates & Helpers

        private unsafe void UpdateDepth()
        {
            lock (this)
            {
                // Get information about the depth image
                DepthMetaData depthMD = new DepthMetaData();

                // Bitmap sperren
                Rectangle rect = new Rectangle(0, 0, this.bitmap.Width, this.bitmap.Height);
                BitmapData data = this.bitmap.LockBits(rect, ImageLockMode.WriteOnly, System.Drawing.Imaging.PixelFormat.Format24bppRgb);

                depthGen.GetMetaData(depthMD);

                // Pointer zum depth image
                //UInt16MapData map = this.depth.GetDepthMap();
                ushort* map = (ushort*)this.depthGen.DepthMapPtr.ToPointer();

                // über Tiefenbild iterieren und Bitmap setzen
                for (int y = 0; y < depthMD.YRes; ++y)
                {
                    byte* pDest = (byte*)data.Scan0.ToPointer() + y * data.Stride;
                    for (int x = 0; x < depthMD.XRes; ++x, ++map, pDest += 3)
                    {
                        pDest[0] = (byte)(*map >> 2);
                        pDest[1] = (byte)(*map >> 3);
                        pDest[2] = (byte)(*map >> 4);
                    }
                }

                this.bitmap.UnlockBits(data);

                // Update the image to have the bitmap image we just copied
                this.picBox.Image = getBitmapImage(bitmap);
            }
        }

        // eigene Erweiterung, um zwischen Tiefenbild und Farbbild umzuschalten
        private unsafe void UpdateRGB()
        {
            lock (this)
            {
                ImageMetaData imgMD = new ImageMetaData();

                Rectangle rect = new Rectangle(0, 0, this.bitmap.Width, this.bitmap.Height);
                BitmapData data = this.bitmap.LockBits(rect, ImageLockMode.WriteOnly, System.Drawing.Imaging.PixelFormat.Format24bppRgb);

                rgbGen.GetMetaData(imgMD);

                byte* map = (byte*)this.rgbGen.ImageMapPtr.ToPointer();

                for (int y = 0; y < imgMD.YRes; ++y)
                {
                    byte* pDest = (byte*)data.Scan0.ToPointer() + y * data.Stride;
                    for (int x = 0; x < imgMD.XRes; ++x, map += 3, pDest += 3)
                    {
                        pDest[0] = map[2];
                        pDest[1] = map[1];
                        pDest[2] = map[0];
                    }
                }

                this.bitmap.UnlockBits(data);

                this.picBox.Image = getBitmapImage(bitmap);

            }
        }

        private Image getBitmapImage(Bitmap bitmap)
        {
            MemoryStream ms = new MemoryStream();
            bitmap.Save(ms, ImageFormat.Png);
            ms.Position = 0;
            Image im = Image.FromStream(ms);
            return im;
        }

        #endregion
    }
}
